/*
 *  FIR filter implementation
 *
 *  splib.lib
 */

#include "fir.h"

void firInit(float* buff, int* idx, const int NPOW2)
{
    int i;
    for(i = 0; i < NPOW2; ++i)
    {
        buff[i] = 0;
    }
    *idx = 0;
}

void firLMS(float* coeff, const float* buff, int idx, const float e, const float mu, const int NPOW2)
{
    int i;
    float x;
    for(i = 0; i < NPOW2; ++i)
    {
        x = buff[(idx + i) & (NPOW2-1)];
        coeff[i] += 2.0f*mu*e*x;
    }
}

void firLMS_DP(double* coeff, const float* buff, int idx, const float e, const float mu, const int NPOW2)
{
    int i;
    float x;
    for(i = 0; i < NPOW2; ++i)
    {
        x = buff[(idx + i) & (NPOW2-1)];
        coeff[i] += 2.0*mu*e*x;
    }
}

float firDelay(float input, float* buff, int* idx, const int NPOW2)
{
    float z = buff[*idx];
    buff[*idx] = input;    
    *idx = (*idx + 1) & (NPOW2-1);
    return z;
}

float firRun(const float input, float* buff, const float* coeff, int* idx, const int NPOW2)
{
    double acc;
    int i;

    buff[*idx] = input;    
    *idx = (*idx + 1) & (NPOW2-1);

    acc = 0;
    for(i = 0; i < NPOW2; ++i)
        acc += buff[(*idx + i) & (NPOW2-1)] * coeff[i];

    return (float)acc;    
}

float firRun_DP(const float input, float* buff, const double* coeff, int* idx, const int NPOW2)
{
    double acc;
    int i;

    buff[*idx] = input;    
    *idx = (*idx + 1) & (NPOW2-1);
#if 1
    acc = 0;
    for(i = 0; i < NPOW2; ++i)
        acc += buff[(*idx + i) & (NPOW2-1)] * coeff[i];
#else
    acc = buff[(*idx + (NPOW2/2)-1) & (NPOW2-1)] * 0.5;
#endif
    return (float)acc;    
}

void firInit2(float* buff2n, int* idx, const int n)
{
    int i;
    for(i = 0; i < 2*n; ++i)
    {
        buff2n[i] = 0;
    }
    *idx = 0;
}

float firRun2(const float input, float* buff2n, const float* coeff, int* idx, const int n)
{
    double acc;
    int i;

    buff2n[*idx  ] = input;    
    buff2n[*idx+n] = input;    
    if(++(*idx) >= n)
        *idx = 0;

    acc = 0;
    for(i = 0; i < n; ++i)
        acc += buff2n[*idx + i] * coeff[i];

    return (float)acc;    
}
